#include <math.h>
#include <stm32f10x_gpio.h>
#include "stm32_delay.h"
#include "stm32_engine.h"
#include "stm32_jy901.h"
#include "stm32_usart3.h"
#include "stm32_led.h"

#define BLUETHOOT_CMD_NONE	0x01
#define BLUETHOOT_CMD_SET_ANGLE	0x02
#define BLUETHOOT_CMD_SET_LENGHT	0x03
#define BLUETHOOT_CMD_FUN_12	0x04
#define BLUETHOOT_CMD_FUN_3	0x05
#define BLUETHOOT_CMD_FUN_4	0x06
#define BLUETHOOT_CMD_FUN_56	0x07
#define BLUETHOOT_CMD_STOP	0x08

#define ANGLE_SPEED_DIV	0.0010652644358317056
#define ANGLE_DIV	0.00009587379922485352
#define STD_ANGLE_X	(0.0853f)
#define STD_ANGLE_Y	(-0.0078f)
#define STD_ANGLE_Z	0.0f
#define STD_ANGLE_SPEED_X	(0.0128f)
#define STD_ANGLE_SPEED_Y	(0.03f)
#define STD_ANGLE_SPEED_Z	0.0f
#define ENGINE_DIRECTION_DOWN	2
#define ENGINE_DIRECTION_UP	1
#define ENGINE_DIRECTION_LEFT	0
#define ENGINE_DIRECTION_RIGHT	3
Jy901Struct *j;
static float lenght = 0.0f;
static float angle = 0.0f;
uint8_t	cmd = 0x01;
NVIC_InitTypeDef tim2_nvic_init_struct;
void stm32_tim2_init(){
	TIM_TimeBaseInitTypeDef tim_timebase_init_struct;
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);
	tim2_nvic_init_struct.NVIC_IRQChannel = TIM2_IRQn;
	tim2_nvic_init_struct.NVIC_IRQChannelPreemptionPriority = 0;
	tim2_nvic_init_struct.NVIC_IRQChannelSubPriority = 4;
	tim2_nvic_init_struct.NVIC_IRQChannelCmd = ENABLE;
	TIM_DeInit(TIM2);                              //复位TIM3定时器
	tim_timebase_init_struct.TIM_Period = 20;        // 100ms     
	tim_timebase_init_struct.TIM_Prescaler = 36000;    // 分频36000       
	tim_timebase_init_struct.TIM_ClockDivision = TIM_CKD_DIV1;  // 时钟分频  
	tim_timebase_init_struct.TIM_CounterMode = TIM_CounterMode_Up;  //计数方向向上计数
	TIM_TimeBaseInit(TIM2, &tim_timebase_init_struct);
}
void stm32_tim2_start(){
	TIM_ClearFlag(TIM2, TIM_FLAG_Update);
	TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);
	TIM_Cmd(TIM2, ENABLE);
	tim2_nvic_init_struct.NVIC_IRQChannelCmd = ENABLE;
	NVIC_Init(&tim2_nvic_init_struct);
}

void CountAngleData(Jy901Struct *j,float *angle_x,float *angle_y,float *angle_speed_x,float *angle_speed_y){
	*angle_x = j->angle.x * ANGLE_DIV + STD_ANGLE_X;
	*angle_y = j->angle.y * ANGLE_DIV + STD_ANGLE_Y;
	*angle_speed_x = j->speed.x * ANGLE_SPEED_DIV + STD_ANGLE_SPEED_X;
	*angle_speed_y = j->speed.y * ANGLE_SPEED_DIV + STD_ANGLE_SPEED_Y;
}
float CountPower(float angle_x,float angle_y,float angle_speed_x,float angle_speed_y){
	return 0.0818*angle_speed_x*angle_speed_x + 6.8469*(1-cos(angle_x))+0.0818*angle_speed_y*angle_speed_y + 6.8469*(1-cos(angle_y));

}

float CountStdPower(float r){
	return 6.8469*(1-0.9165/sqrt(r*r+0.83997225));
}
void CtrlEngine3_def(Jy901Struct *j,float angle,float r){
	static float angle_x,angle_y,angle_speed_x,angle_speed_y,power_x,power_y;
	static float power_std =  0.06f,power,power_dt;
	CountAngleData(j,&angle_x,&angle_y,&angle_speed_x,&angle_speed_y);
	power = CountPower(angle_x,angle_y,angle_speed_x,angle_speed_y);
	power_std = CountStdPower(r);
	power_dt = 1-power/power_std;
	if(power_dt>0){
		power_x = power_dt*cos(angle);
		power_y = power_dt*sin(angle);
		if(power_x>0){
		//第一象限
			if(angle_speed_y<0&&angle_speed_x>0){
			//拉动
				stm32_engine_set_cnv(power_x,ENGINE_DIRECTION_RIGHT);
				stm32_engine_set_cnv(power_y,ENGINE_DIRECTION_DOWN);
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_LEFT);
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_UP);
			}else{
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_DOWN);
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_RIGHT);
				stm32_engine_set_cnv(power_y,ENGINE_DIRECTION_UP);
				stm32_engine_set_cnv(power_x,ENGINE_DIRECTION_LEFT);
			}
		}else{
		//第二象限
			if(angle_speed_y<0){
			//拉动
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_RIGHT);
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_UP);
				stm32_engine_set_cnv(fabs(power_x),ENGINE_DIRECTION_LEFT);
				stm32_engine_set_cnv(fabs(power_y),ENGINE_DIRECTION_DOWN);
			}else{
			//回荡
				stm32_engine_set_cnv(fabs(power_x),ENGINE_DIRECTION_RIGHT);
				stm32_engine_set_cnv(fabs(power_y),ENGINE_DIRECTION_UP);
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_LEFT);
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_DOWN);
			
			}
		}
	}else{
		stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_RIGHT);
		stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_UP);
		stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_LEFT);
		stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_DOWN);
		stm32_led_on();
	}
}
void CtrlEngine3(Jy901Struct *j,float angle){
	static float angle_x,angle_y,angle_speed_x,angle_speed_y,power_x,power_y;
	static float power_std =  0.0f,power,power_dt;
	CountAngleData(j,&angle_x,&angle_y,&angle_speed_x,&angle_speed_y);
	power = CountPower(angle_x,angle_y,angle_speed_x,angle_speed_y);
	power_std = CountStdPower(lenght);
	power_dt = 1-power/power_std;
	if(power_dt>0){
		power_x = power_dt*cos(angle);
		power_y = power_dt*sin(angle);
		if(power_x>0){
		//第一象限
			if(angle_speed_y<0){
			//拉动
				stm32_engine_set_cnv(fabs(power_x),ENGINE_DIRECTION_RIGHT);
				stm32_engine_set_cnv(fabs(power_y),ENGINE_DIRECTION_DOWN);
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_LEFT);
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_UP);
			}else{
			//回荡
				if(fabs(angle_x)<0.02){
				//与Y相交
					if(angle_y>0){
						stm32_engine_set_cnv(0.5,ENGINE_DIRECTION_DOWN);
						stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_UP);
					}else{
						stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_DOWN);
						stm32_engine_set_cnv(0.5,ENGINE_DIRECTION_UP);
					}
					stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_LEFT);
					stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_RIGHT);
				}else if(fabs(angle_y)<0.02){
				//与X相交`
					if(angle_x>0){
						stm32_engine_set_cnv(0.5,ENGINE_DIRECTION_LEFT);
						stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_RIGHT);
					}else{
						stm32_engine_set_cnv(0.5,ENGINE_DIRECTION_RIGHT);
						stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_LEFT);
					}
					stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_DOWN);
					stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_UP);
				}else{
					stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_DOWN);
					stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_RIGHT);
					stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_UP);
					stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_LEFT);
				}
			}
		}else{
		//第二象限
			if(angle_speed_y<0){
			//拉动
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_RIGHT);
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_UP);
				stm32_engine_set_cnv(fabs(power_x),ENGINE_DIRECTION_LEFT);
				stm32_engine_set_cnv(fabs(power_y),ENGINE_DIRECTION_DOWN);
			}else{
			//回荡
				stm32_engine_set_cnv(fabs(power_x),ENGINE_DIRECTION_RIGHT);
				stm32_engine_set_cnv(fabs(power_y),ENGINE_DIRECTION_UP);
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_LEFT);
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_DOWN);
			
			}
		}
	}else{
		stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_RIGHT);
		stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_UP);
		stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_LEFT);
		stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_DOWN);
		stm32_led_on();
	}
}

void CtrlEngine4(Jy901Struct *j){
	static float angle_x,angle_y,angle_speed_x,angle_speed_y,power;
	CountAngleData(j,&angle_x,&angle_y,&angle_speed_x,&angle_speed_y);
	if(fabs(angle_speed_x)>0.17){
		if(angle_speed_x>0){
			stm32_engine_set_cnv(1.0f,ENGINE_DIRECTION_LEFT);
			stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_RIGHT);
		}else{
			stm32_engine_set_cnv(1.0f,ENGINE_DIRECTION_RIGHT);	
			stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_LEFT);	
		}
	}else{
		stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_RIGHT);
		stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_LEFT);	
	}
	if(fabs(angle_speed_y)>0.17){
		if(angle_speed_y>0){
			stm32_engine_set_cnv(1.0f,ENGINE_DIRECTION_DOWN);	
			stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_UP);	
		}else{
			stm32_engine_set_cnv(1.0f,ENGINE_DIRECTION_UP);	
			stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_DOWN);	
		}
	}else{
		stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_UP);	
		stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_DOWN);	
	}
}
void CtrlEngine12(Jy901Struct *j,float r){
	static float power_std = 0.0000f,power,power_dt;
	static float angle_x,angle_y,angle_speed_x,angle_speed_y;
	CountAngleData(j,&angle_x,&angle_y,&angle_speed_x,&angle_speed_y);
	power_std = CountStdPower(r);
	power = CountPower(angle_x,0.0f,angle_speed_x,0.0f);
	power_dt = 1-power/power_std;
	if(power_dt>0){
		if(angle_speed_x<0){
			stm32_engine_set_cnv(power_dt,ENGINE_DIRECTION_LEFT);
			stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_RIGHT);
		}else{
			stm32_engine_set_cnv(power_dt,ENGINE_DIRECTION_RIGHT);
			stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_LEFT);
		}
	}else{
		stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_RIGHT);
		stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_LEFT);
		stm32_led_on();
	}
}

void CtrlEngine56(Jy901Struct *j,float r){
	static float power_std = 0.0000f,power,power_dt;
	static float angle_x,angle_y,angle_speed_x,angle_speed_y,angle_x_abs,angle_y_abs;
	CountAngleData(j,&angle_x,&angle_y,&angle_speed_x,&angle_speed_y);
	power_std = CountStdPower(r);
	power = CountPower(0.0f,0.0f,angle_speed_x,angle_speed_y);
	power_dt = 1-power/power_std;
	if(power_dt>0){
		angle_x_abs = fabs(angle_x);
		angle_y_abs = fabs(angle_x);
		if(angle_x_abs<0.05){
			if(angle_y>0){
				stm32_engine_set_cnv(power_dt*0.9,ENGINE_DIRECTION_LEFT);
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_RIGHT);
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_DOWN);
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_UP);
			}else{
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_LEFT);
				stm32_engine_set_cnv(power_dt*0.9,ENGINE_DIRECTION_RIGHT);
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_DOWN);
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_UP);
			}
		}else if(angle_y_abs<0.05){
			if(angle_x>0){
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_LEFT);
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_RIGHT);
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_DOWN);
				stm32_engine_set_cnv(power_dt,ENGINE_DIRECTION_UP);
			}else{
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_LEFT);
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_RIGHT);
				stm32_engine_set_cnv(power_dt,ENGINE_DIRECTION_DOWN);
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_UP);
			}
			
		
		}else if(fabs(angle_x_abs-angle_y_abs)<0.05){
			power_dt = power_dt*0.7071;
			if(angle_x>0&&angle_y>0){
				stm32_engine_set_cnv(power_dt*0.9,ENGINE_DIRECTION_LEFT);
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_RIGHT);
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_DOWN);
				stm32_engine_set_cnv(power_dt,ENGINE_DIRECTION_UP);
			
			}else if(angle_x>0&&angle_y<0){
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_LEFT);
				stm32_engine_set_cnv(power_dt*0.9,ENGINE_DIRECTION_RIGHT);
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_DOWN);
				stm32_engine_set_cnv(power_dt,ENGINE_DIRECTION_UP);
			
			}else if(angle_x<0&&angle_y>0){
				stm32_engine_set_cnv(power_dt*0.9,ENGINE_DIRECTION_LEFT);
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_RIGHT);
				stm32_engine_set_cnv(power_dt,ENGINE_DIRECTION_DOWN);
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_UP);
			
			}else if(angle_x<0&&angle_y<0){
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_LEFT);
				stm32_engine_set_cnv(power_dt*0.9,ENGINE_DIRECTION_RIGHT);
				stm32_engine_set_cnv(power_dt,ENGINE_DIRECTION_DOWN);
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_UP);
			
			}
		}
	}else{
		stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_LEFT);
		stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_RIGHT);
		stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_DOWN);
		stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_UP);
		stm32_led_on();
	}
}
void TIM2_IRQHandler(){
	static float angle_x,angle_y,angle_speed_x,angle_speed_y,power,power_dt;
	if(TIM_GetITStatus(TIM2, TIM_IT_Update) != RESET){
		j = stm32_jy901_get();
		switch(cmd){
			case BLUETHOOT_CMD_FUN_12:
				CtrlEngine12(j,lenght);
			break;
			case BLUETHOOT_CMD_FUN_3:
				CtrlEngine3_def(j,angle,lenght);
			break;
			case BLUETHOOT_CMD_FUN_4:
				CtrlEngine4(j);
			break;
			case BLUETHOOT_CMD_FUN_56:
				CtrlEngine56(j,lenght);
			break;
			default:
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_UP);
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_DOWN);
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_LEFT);
				stm32_engine_set_cnv(0.0f,ENGINE_DIRECTION_RIGHT);
				stm32_led_off();
			break;
		}
		TIM_ClearITPendingBit(TIM2, TIM_FLAG_Update);
	}
}

float angle_sin_bc(float value){
	if(value<0.5235987755f){
		value+=0.13962634*sin(value*6);
	}else if(value<1.047197551f){
		value-=0.13962634013*sin((value-0.5235987755)*3);
	}else if(value<1.5707963265f){
		value-=0.26179938775*sin((value-0.5235987755)*3);
	}else if(value<2.4434609523f){
		value+=0.20943951020*sin((value-1.5707963265)*3.5);
	}else{
		value-=0.06981317007*sin((value-2.4434609523)*4.5);
	}
	return value;
}

int main(){
	uint8_t c=0x00;
	uint8_t val = 0x00;
	stm32_led_init();
	stm32_engine_init();
	stm32_usart3_init();
	stm32_jy901_init();
	stm32_tim2_init();
	stm32_tim2_start();
	while(1){
		c = stm32_usart3_read();
		stm32_usart3_write(c);
		val = stm32_usart3_read();
		switch(c){
			case BLUETHOOT_CMD_SET_ANGLE:
				angle = angle_sin_bc((float)val/180.0*3.141592653);
			break;
			case BLUETHOOT_CMD_SET_LENGHT:
				lenght = (((float)val-30.0f)*0.8333333333333334+20)/200.0;
			break;
			default:
				cmd = c;
			break;
		}
		stm32_usart3_write(j->angle.x);
		stm32_usart3_write(j->angle.x>>8);
		stm32_usart3_write(j->angle.y);
		stm32_usart3_write(j->angle.y>>8);
		stm32_usart3_write(j->angle.z);
		stm32_usart3_write(j->angle.z>>8);
		stm32_usart3_write(j->speed.x);
		stm32_usart3_write(j->speed.x>>8);
		stm32_usart3_write(j->speed.y);
		stm32_usart3_write(j->speed.y>>8);
		stm32_usart3_write(j->speed.z);
		stm32_usart3_write(j->speed.z>>8);
	}
	while(1);
	return 0;
}
